#include "math_utils.h"

namespace math_utils{

/************************************
 * 角度计算
 * https://wiki.ros.org/angles
 *************************************/

// 弧度转角度
float Rad2Deg(float radians){
    return radians * 180.0 / M_PI;
}

// 角度转弧度
float Deg2Rad(float degrees){
    return degrees * M_PI / 180.0;
}

// 角度转换 [-PI,PI]
float NorMalizeAngle(float angle){
  const float result = fmod(angle + M_PI, 2.0 * M_PI);
  if (result <= 0.0)
    return result + M_PI;
  return result - M_PI;
}

// 角度差
float DifferenceAngle(float a, float b){
    return NorMalizeAngle(b - a);
}

// 角度相加
float AdditionAngle(float a, float b){
    return NorMalizeAngle(a + b);
}

/************************************
 * 位姿计算
 *************************************/
// 计算两点之间距离
float DistancePose2D(Pose2D a, Pose2D b){
    float dx = b.x - a.x;
    float dy = b.y - a.y;
    return hypot(dx, dy);
}

// 计算两点之间角度 [-PI,PI]
float AnglePose2D(Pose2D a, Pose2D b){
    float dx = b.x - a.x;
    float dy = b.y - a.y;
    return atan2(dy, dx);
}



// 位姿转换 Map -> Base 坐标
geometry_msgs::TransformStamped Map2BaseTransform(Pose2D curr){
    auto transform = msg_convert::Pose2D2TransformStamped(curr);
    tf2::Transform transform_tf = msg_convert::Geometry2Transform(transform);
    tf2::Transform inverse_transform = transform_tf.inverse();
    return msg_convert::Transform2Geometry(inverse_transform);
}

// 位姿转换 Base -> Map 坐标
geometry_msgs::TransformStamped Base2MapTransform(Pose2D curr){
    return msg_convert::Pose2D2TransformStamped(curr);
}

// 位姿转换
Pose2D Pose2DGlobal2Local(Pose2D curr, Pose2D global_pose){
    Pose2D local_pose;
    float delta_x = global_pose.x - curr.x;
    float delta_y = global_pose.y - curr.y;
    float cos_yaw = cos(-curr.yaw);
    float sin_yaw = sin(-curr.yaw);
    local_pose.x = delta_x * cos_yaw - delta_y * sin_yaw;
    local_pose.y = delta_x * sin_yaw + delta_y * cos_yaw;
    local_pose.yaw = DifferenceAngle(curr.yaw, global_pose.yaw);
    return local_pose;
}

// 位姿转换
Pose2D Pose2DLocal2Global(Pose2D curr, Pose2D local_pose){
    Pose2D global_pose;
    float cos_yaw = std::cos(curr.yaw);
    float sin_yaw = std::sin(curr.yaw);
    global_pose.x = curr.x + local_pose.x * cos_yaw - local_pose.y * sin_yaw;
    global_pose.y = curr.y + local_pose.x * sin_yaw + local_pose.y * cos_yaw;
    global_pose.yaw = AdditionAngle(curr.yaw, local_pose.yaw);
    return global_pose;
}

// Path 路径转换
vector<Pose2D> PathGlobal2Local(Pose2D curr, vector<Pose2D>& points){
    vector<Pose2D> local_points;
    float cos_yaw = cos(-curr.yaw);
    float sin_yaw = sin(-curr.yaw);
    for(auto global_pose : points){
        Pose2D local_pose;
        float delta_x = global_pose.x - curr.x;
        float delta_y = global_pose.y - curr.y;
        local_pose.x = delta_x * cos_yaw - delta_y * sin_yaw;
        local_pose.y = delta_x * sin_yaw + delta_y * cos_yaw;
        local_pose.yaw = DifferenceAngle(curr.yaw, global_pose.yaw);
        local_points.emplace_back(local_pose);
    }
    return local_points;
}



}; //math_utils 